#include "z1arm.h"
#include "globals.h"  // 可以使用 exit_flag, arm_mutex, etc.

using namespace UNITREE_ARM;

Z1ARM::Z1ARM() : unitreeArm(true), tcp_sockfd(-1) {
    gripper_pos = 0.0;
    joint_speed = 2.0;
    cartesian_speed = 3.0;
}

Z1ARM::~Z1ARM()
{
    if(tcp_sockfd != -1){
        close(tcp_sockfd);
        std::cout << "[~Z1ARM] TCP socket closed." << std::endl;
    }
}

void Z1ARM::armCtrlByFSM() {
    std::cout << "[armCtrlByFSM] Before labelRun(\"forward\")" << std::endl;
    try {
        labelRun("forward");
        std::cout << "[armCtrlByFSM] labelRun(\"forward\") OK." << std::endl;
    }
    catch (...) {
        std::cerr << "[armCtrlByFSM] Exception in labelRun." << std::endl;
        return;
    }

    std::cout << "[armCtrlByFSM] After labelRun" << std::endl;
    printState();

    {
        std::lock_guard<std::mutex> lock(arm_mutex);
        forward_position = lowstate->endPosture;
        std::cout << "[armCtrlByFSM] Save forward_position: " 
                  << forward_position.transpose() << std::endl;
    }
}

void Z1ARM::armCtrlInJointCtrl() {
    // 省略: 复制你原先的逻辑即可
    std::cout << "[Z1ARM::armCtrlInJointCtrl] ...\n";
}
void Z1ARM::armCtrlInCartesian() {
    // 省略: 复制你原先的逻辑即可
    std::cout << "[Z1ARM::armCtrlInCartesian] ...\n";
}

void Z1ARM::printState()
{
    std::cout << "------ joint State ------" << std::endl;
    std::cout << "qState: " << lowstate->getQ().transpose() << std::endl;
    std::cout << "qdState: " << lowstate->getQd().transpose() << std::endl;
    std::cout << "tauState: " << lowstate->getTau().transpose() << std::endl;
    std::cout << "------ Endeffector Cartesian Posture ------" << std::endl;
    
    // 设置输出精度为小数点后6位
    std::cout << std::fixed << std::setprecision(6);
    std::cout << "(roll pitch yaw x y z) = " << lowstate->endPosture.transpose() << std::endl;
    
    // 恢复默认精度（可选）
    std::cout << std::defaultfloat;
}
